{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "31f395c5",
      "metadata": {},
      "source": [
        "# CustomFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "1a3591a2",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/nonlinear/doc/CustomFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "5ccb48e4",
      "metadata": {
        "tags": [
          "remove-cell"
        ],
        "vscode": {
          "languageId": "markdown"
        }
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "10df70c9",
      "metadata": {},
      "source": [
        "\n",
        "## Overview\n",
        "\n",
        "The `CustomFactor` class allows users to define custom error functions and Jacobians, and while it can be used in C++, it is particularly useful for use with the python wrapper.\n",
        "\n",
        "## Custom Error Function\n",
        "\n",
        "The `CustomFactor` class allows users to define a custom error function. In C++ it is defined as below:\n",
        "\n",
        "```cpp\n",
        "using JacobianVector = std::vector<Matrix>;\n",
        "using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;\n",
        "```\n",
        "\n",
        "The function will be passed a reference to the factor itself so the keys can be accessed, a `Values` reference, and a writeable vector of Jacobians.\n",
        "\n",
        "## Usage in Python\n",
        "\n",
        "In order to use a Python-based factor, one needs to have a Python function with the following signature:\n",
        "\n",
        "```python\n",
        "def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray]) -> np.ndarray:\n",
        "    ...\n",
        "```\n",
        "\n",
        "**Explanation**:\n",
        "- `this` is a reference to the `CustomFactor` object. This is required because one can reuse the same `error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of *references* to the list of required Jacobians (see the corresponding C++ documentation). \n",
        "- the error returned must be a 1D `numpy` array.\n",
        "- If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error`\n",
        "method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`,\n",
        "each entry of `H` can be assigned a (2D) `numpy` array, as the Jacobian for the corresponding variable.\n",
        "- All `numpy` matrices inside should be using `order=\"F\"` to maintain interoperability with C++.\n",
        "\n",
        "After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM. In summary, to use `CustomFactor`, users must:\n",
        "1. Define the custom error function that models the specific measurement or constraint.\n",
        "2. Implement the calculation of the Jacobian matrix for the error function.\n",
        "3. Define a noise model of the appropriate dimension.\n",
        "3. Add the `CustomFactor` to a factor graph, specifying\n",
        "    - the noise model\n",
        "    - the keys of the variables it depends on\n",
        "    - the error function"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "c7ec3512",
      "metadata": {},
      "source": [
        "**Notes**:\n",
        "- There are not a lot of restrictions on the function, but note there is overhead in calling a python function from within a c++ optimization loop. \n",
        "- Because `pybind11` needs to lock the Python GIL lock for evaluation of each factor, parallel evaluation of `CustomFactor` is not possible.\n",
        "- You can mitigate both of these by having a python function that leverages batching of measurements.\n",
        "\n",
        "Some more examples of usage in python are given in [test_custom_factor.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/tests/test_custom_factor.py),[CustomFactorExample.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/CustomFactorExample.py), and [CameraResectioning.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/CameraResectioning.py)."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "68a66627",
      "metadata": {},
      "source": [
        "## Example\n",
        "Below is a simple example that mimics a `BetweenFactor<Pose2>`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "894bfaf2",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "CustomFactor on 66, 77\n",
            "isotropic dim=3 sigma=0.1\n",
            "\n"
          ]
        }
      ],
      "source": [
        "import numpy as np\n",
        "from gtsam import CustomFactor, noiseModel, Values, Pose2\n",
        "\n",
        "measurement = Pose2(2, 2, np.pi / 2) # is used to create the error function\n",
        "\n",
        "def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]=None):\n",
        "    \"\"\"\n",
        "    Error function that mimics a BetweenFactor\n",
        "    :param this: reference to the current CustomFactor being evaluated\n",
        "    :param v: Values object\n",
        "    :param H: list of references to the Jacobian arrays\n",
        "    :return: the non-linear error\n",
        "    \"\"\"\n",
        "    key0 = this.keys()[0]\n",
        "    key1 = this.keys()[1]\n",
        "    gT1, gT2 = v.atPose2(key0), v.atPose2(key1)\n",
        "    error = measurement.localCoordinates(gT1.between(gT2))\n",
        "\n",
        "    if H is not None:\n",
        "        result = gT1.between(gT2)\n",
        "        H[0] = -result.inverse().AdjointMap()\n",
        "        H[1] = np.eye(3)\n",
        "    return error\n",
        "\n",
        "# we use an isotropic noise model, and keys 66 and 77\n",
        "noise_model = noiseModel.Isotropic.Sigma(3, 0.1)\n",
        "custom_factor = CustomFactor(noise_model, [66, 77], error_func)\n",
        "print(custom_factor)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "b72a8fc7",
      "metadata": {},
      "source": [
        "Typically, you would not actually call methods of a custom factor directly: a nonlinear optimizer will call `linearize` in every nonlinear iteration. But if you wanted to, here is how you would do it:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "c92caf2c",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "error =  0.0\n",
            "Linearized JacobianFactor:\n",
            "   A[66] = [\n",
            "\t-6.12323e-16, -10, -20;\n",
            "\t10, -6.12323e-16, -20;\n",
            "\t-0, -0, -10\n",
            "]\n",
            "  A[77] = [\n",
            "\t10, 0, 0;\n",
            "\t0, 10, 0;\n",
            "\t0, 0, 10\n",
            "]\n",
            "  b = [ -0 -0 -0 ]\n",
            "  No noise model\n",
            "\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "values.insert(66, Pose2(1, 2, np.pi / 2))\n",
        "values.insert(77, Pose2(-1, 4, np.pi))\n",
        "\n",
        "print(\"error = \", custom_factor.error(values))\n",
        "print(\"Linearized JacobianFactor:\\n\", custom_factor.linearize(values))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "d9b61f83",
      "metadata": {},
      "source": [
        "## Beware of Jacobians!\n",
        "\n",
        "It is important to unit-test the Jacobians you provide, because the convention used in GTSAM frequently leads to confusion. In particular, GTSAM updates variables using an exponential map *on the right*. In particular, for a variable $x\\in G$, an n-dimensional Lie group, the Jacobian $H_a$ at $x=a$ is defined as the linear map satisfying\n",
        "$$\n",
        "\\lim_{\\xi\\rightarrow0}\\frac{\\left|f(a)+H_a\\xi-f\\left(a \\, \\text{Exp}(\\xi)\\right)\\right|}{\\left|\\xi\\right|}=0,\n",
        "$$\n",
        "where $\\xi$ is a n-vector corresponding to an element in the Lie algebra $\\mathfrak{g}$, and $\\text{Exp}(\\xi)\\doteq\\exp(\\xi^{\\wedge})$, with $\\exp$ the exponential map from $\\mathfrak{g}$ back to $G$. The same holds for n-dimensional manifold $M$, in which case we use a suitable retraction instead of the exponential map. More details and examples can be found in [doc/math.pdf](https://github.com/borglab/gtsam/blob/develop/gtsam/doc/math.pdf).\n",
        "\n",
        "To test your Jacobians, you can use the handy `gtsam.utils.numerical_derivative` module. We give an example below:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "c815269f",
      "metadata": {},
      "outputs": [],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative21, numericalDerivative22\n",
        "\n",
        "# Allocate the Jacobians and call error_func\n",
        "H = [np.empty((6, 6), order='F'),np.empty((6, 6), order='F')]\n",
        "error_func(custom_factor, values, H)\n",
        "\n",
        "# We use error_func directly, so we need to create a binary function constructing the values.\n",
        "def f (T1, T2):\n",
        "    v = Values()\n",
        "    v.insert(66, T1)\n",
        "    v.insert(77, T2)\n",
        "    return error_func(custom_factor, v)\n",
        "numerical0 = numericalDerivative21(f, values.atPose2(66), values.atPose2(77))\n",
        "numerical1 = numericalDerivative22(f, values.atPose2(66), values.atPose2(77))\n",
        "\n",
        "# Check the numerical derivatives against the analytical ones\n",
        "np.testing.assert_allclose(H[0], numerical0, rtol=1e-5, atol=1e-8)\n",
        "np.testing.assert_allclose(H[1], numerical1, rtol=1e-5, atol=1e-8)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "fd09b0fc",
      "metadata": {},
      "source": [
        "## Implementation Notes\n",
        "\n",
        "`CustomFactor` is a `NonlinearFactor` that has a `std::function` as its callback.\n",
        "This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.\n",
        "\n",
        "The constructor of `CustomFactor` is\n",
        "```cpp\n",
        "/**\n",
        "* Constructor\n",
        "* @param noiseModel shared pointer to noise model\n",
        "* @param keys keys of the variables\n",
        "* @param errorFunction the error functional\n",
        "*/\n",
        "CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :\n",
        "  Base(noiseModel, keys) {\n",
        "  this->error_function_ = errorFunction;\n",
        "}\n",
        "```\n",
        "\n",
        "At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.\n",
        "\n",
        "Something that deserves a special mention is this:\n",
        "```cpp\n",
        "/*\n",
        " * NOTE\n",
        " * ==========\n",
        " * pybind11 will invoke a copy if this is `JacobianVector &`,\n",
        " * and modifications in Python will not be reflected.\n",
        " *\n",
        " * This is safe because this is passing a const pointer, \n",
        " * and pybind11 will maintain the `std::vector` memory layout.\n",
        " * Thus the pointer will never be invalidated.\n",
        " */\n",
        "using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;\n",
        "```\n",
        "which is not documented in `pybind11` docs. One needs to be aware of this if they wanted to implement similar \"mutable\" arguments going across the Python-C++ boundary.\n"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}